#include "auxiliary.h"

// json 传递定义
std::unordered_map<std::string, std::vector<double>> hs;

// 2. 全局变量 hs 的声明
static std::mutex hs_mutex;  // 保护 hs 的线程安全（定时器多线程访问）


std::string toHexString(const std::string& data) {
    static const char* hexChars = "0123456789ABCDEF";
    std::string result;
    result.reserve(data.size() * 3); // 每个字节占2个字符+1个空格

    for (unsigned char c : data) {
        result += hexChars[(c >> 4) & 0xF];
        result += hexChars[c & 0xF];
        result += ' ';
    }

    return result;
}

std::string transfer_json(const std::string& name, const std::vector<double>& data)
{
    nlohmann::json j;
    j["sensor_type"] = name;

    if (name == COMPASS) {
        j["data"] = transfer_compass_json(data);
    }
    else if (name == DEPTH) {
        j["data"] = transfer_depth_json(data);
    }
    else if (name == DVL) {
        j["data"] = transfer_dvl_json(data);
    }
    else if (name == GUIDANCE) {
        j["data"] = transfer_guidance_json(data);
    }
    else if (name == GPS) {
        j["data"] = transfer_gps_json(data);
    }
    else if (name == POSTURE) {
        j["data"] = transfer_posture_json(data);
    }
    else if (name == SEATRAC) {
        j["data"] = transfer_seatrac_json(data);
    }
    else if (name == HUMITURE) {
        j["data"] = transfer_humiture_json(data);
    }

    return j.dump(4);
}

nlohmann::json transfer_compass_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::vector<std::string> name_list = { "pitch", "roll", "heading" };

    for (size_t i = 0; i < data.size(); i++) {
        jd[name_list.at(i)] = data.at(i);
    }

    return jd;
}

nlohmann::json transfer_depth_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::string name_list = { "pressure" };

    jd[name_list] = data.at(0);

    return jd;
}

nlohmann::json transfer_dvl_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::vector<std::string> name_list = { "altitude", "coordinate", "direction" };

    jd[name_list.at(0)] = { data.at(3) };
    jd[name_list.at(1)] = { data.at(0), data.at(1), data.at(2) };
    jd[name_list.at(2)] = { data.at(4), data.at(5), data.at(6) };

    //for (size_t i = 1; i < name_list.size(); i++) {
    //    jd[name_list.at(1) + 1] = { data.at(i * 3), data.at(i * 3 + 1), data.at(i * 3 + 2) };
    //}


    return jd;
}

nlohmann::json transfer_guidance_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::vector<std::string> name_list = {
        "ine_eluer", "ine_coordinate", "ine_direction",
        "axis", "velocity",
        "com_eluer", "com_coordinate", "com_direction",
        "sat_coordinate", "sat_direction" };

    for (size_t i = 0; i < name_list.size(); i++) {
        jd[name_list.at(i)] = { data.at(i * 3), data.at(i * 3 + 1), data.at(i * 3 + 2) };
    }
    return jd;
}

nlohmann::json transfer_gps_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::vector<std::string> name_list = { "latitude", "longitude", "speed" };

    //for (size_t i = 0; i < name_list.size(); i++) {
    //    jd[name_list.at(i)] = { data.at(i * 3), data.at(i * 3 + 1), data.at(i * 3 + 2) };
    //}

    jd[name_list.at(0)] = data.at(2);
    jd[name_list.at(1)] = data.at(4);
    jd[name_list.at(2)] = data.at(7);

    return jd;
}

nlohmann::json transfer_posture_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::vector<std::string> name_list = { "acc_velocity", "ang_velocity", "nor_magnet", "raw_magnet", "att_angle" };

    for (size_t i = 0; i < name_list.size(); i++) {
        jd[name_list.at(i)] = { data.at(i * 3), data.at(i * 3 + 1), data.at(i * 3 + 2) };
    }

    return jd;
}

nlohmann::json transfer_humiture_json(const std::vector<double>& data) {
    nlohmann::json jd;
    std::vector<std::string> name_list = { "temperature", "humidity" };

    for (size_t i = 0; i < data.size(); i++) {
        jd[name_list.at(i)] = data.at(i);
    }

    return jd;
}

nlohmann::json transfer_seatrac_json(const std::vector<double>& data)
{
    nlohmann::json jd;
    std::vector<std::string> name_list = { "depth", "supply", "temperature", "yaw", "pitch", "roll" };

    for (size_t i = 37; i < data.size(); i++) {
        jd[name_list.at(i)] = data.at(i);
    }

    return jd;
}

uint8_t calCompassCk(const uint8_t* data, int len)
{
    uint8_t ck = 0;
    for (uint8_t i = 1; i < len - 1; i++) {
        ck = ck + *(data + i);
    }
    return ck;
}

std::pair<uint8_t, uint8_t> calPostureCk(const uint8_t* data, int len)
{
    uint8_t ck1 = 0;
    uint8_t ck2 = 0;

    for (uint16_t i = 0; i < len; i++) {
        ck1 = ck1 + *(data + i);
        ck2 = ck1 + ck2;
    }

    return { ck1, ck2 };
}

uint8_t calGuidanceCk(const uint8_t* data, int len)
{
    uint8_t ck = 0;
    for (uint8_t i = 0; i < len - 1; i++) {
        ck = ck ^ *(data + i);
    }
    return ck;
}

uint16_t calSeatracCk(const uint8_t* data, int len)
{
    uint16_t poly = 0xA001;
    uint16_t crc = 0;
    for (uint16_t b = 0; b < len; b++) {
        uint8_t v = *data;
        for (uint8_t i = 0; i < 8; i++) {
            if ((v & 0x01) ^ (crc & 0x01)) {
                crc >>= 1;
                crc ^= poly;
            }
            else {
                crc >>= 1;
            }
            v >>= 1;
        }
        data++;
    }
    return crc;
}

uint8_t calGPSCK(const std::string& data, size_t start, size_t end)
{
    uint8_t actualChecksum = 0;
    if (start < end && end != std::string::npos) {
        for (size_t i = start; i < end; ++i) {
            actualChecksum ^= data[i];
        }
    }
    return actualChecksum;
}

uint16_t calHumitureCk(const uint8_t* data, int len)
{
    uint16_t crc = 0xFFFF;

    for (int i = 0; i < len; i++) {
        crc ^= data[i];  // 异或低 8 位

        for (int j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001; // 多项式
            }
            else {
                crc >>= 1;
            }
        }
    }

    return crc;
}

double convertCompassFe(const Field& field)
{
    return ((field.functionBit & 0x0f) * 100 +
        ((field.digitBit >> 4) * 10 + (field.digitBit & 0x0f) +
            ((field.decimalBit >> 4) * 10 + (field.decimalBit & 0x0f)) * 0.01) *
        ((field.functionBit >> 4) == 0 ? 1 : -1));
}

std::vector<double> parse_sensor_data(const std::string& name, const std::string& data)
{
    if (name == COMPASS) {
        return parseCompass(data);
    }
    else if (name == DEPTH) {
        return parseDepth(data);
    }
    else if (name == DVL) {
        auto [time, res] = parseDVL(data);
        return res;
    }
    else if (name == GUIDANCE) {
        return parseGuidance(data);
    }
    else if (name == POSTURE) {
        return parsePosture(data);
    }
    else if (name == HUMITURE) {
        return parseHumiture(data);
    }
    else if (name == SEATRAC) {
        return parseSeatrac(data);
    }
    else if (name == GPS) {
        return parseGPS(data);
    }
    return std::vector<double>();
}

std::vector<double> parseCompass(const std::string& data)
{
    uint8_t check;
    bool receive = false;
    uint32_t move = 0;
    std::string compassStr;

    static double pitch = 0;
    static double roll = 0;
    static double heading = 0;


    // 主解析逻辑
    if (!data.empty()) {

        for (size_t i = 0; i < data.size(); i++) {
            if (!receive) {
                if (data[i] == 0x68) {
                    receive = true;
                    compassStr.clear(); // 发现帧头时清空缓冲区
                    compassStr.push_back(data[i]);
                    move = 1;
                }
                continue;
            }
            else {
                compassStr.push_back(data[i]);

                if (++move >= 14) {
                    // 确保有足够的数据进行校验
                    if (move > data.size()) {
                        move = 0;
                        receive = false;
                        compassStr.clear();
                        continue;
                    }

                    const uint8_t* seq = reinterpret_cast<const uint8_t*>(compassStr.data());
                    check = calCompassCk(seq, 14);

                    if (check == static_cast<uint8_t>(data[i])) {
                        // 确保数据长度正确
                        if (compassStr.size() == sizeof(Compass)) {
                            const Compass* compass = reinterpret_cast<const Compass*>(compassStr.data());
                            pitch = convertCompassFe(compass->pitch);
                            roll = convertCompassFe(compass->roll);
                            heading = convertCompassFe(compass->heading);
                        }

                    }
                    move = 0;
                    receive = false;
                    compassStr.clear();
                }
            }
        }
    }

    hs[COMPASS] = { pitch, roll, heading };

    return { pitch, roll, heading };
}

std::vector<double> parseDepth(const std::string& data)
{
    double pressure = 0.0;
    std::string depthStr;
    bool isPressureFound = false;

    for (char c : data) {
        if (c == 'P') {
            isPressureFound = true;
            depthStr.clear(); // 重置字符串
        }
        else if (isPressureFound) {
            if (std::isdigit(c) || c == '.' || c == '-') { // 允许数字、小数点、负号
                depthStr += c;
            }
            else if (!depthStr.empty()) { // 遇到非数字字符且已有数字，结束解析
                break;
            }
        }
    }

    // 转换字符串为数值
    if (!depthStr.empty()) {
        try {
            pressure = std::stod(depthStr);
        }
        catch (const std::invalid_argument& e) {

        }
    }

    hs[DEPTH] = { pressure };

    return { pressure };
}

std::vector<double> parsePosture(const std::string& data)
{
    static double acc_x = 0; static double acc_y = 0; static double acc_z = 0;
    static double ang_x = 0; static double ang_y = 0; static double ang_z = 0;
    static double mag_x = 0; static double mag_y = 0; static double mag_z = 0;
    static double raw_x = 0; static double raw_y = 0; static double raw_z = 0;
    static double pitch = 0; static double roll = 0; static double yaw = 0; static double quat_a = 0;
    static double quat_b = 0; static double quat_c = 0; static double quat_d = 0;
    static double latitude = 0; static double longtitude = 0; static double altitude = 0;
    static double vel_n = 0; static double vel_e = 0; static double vel_d = 0;

    int dataLen = data.size() - 7;


    // 59 53 93 61 3C
    // 10 0C E7 78 00 00 78 3C FD FF 69 6C 95 00 = 14
    // 20 0C F4 E9 03 00 88 23 02 00 70 37 00 00 = 14
    // 40 0C 54 98 FD FF AA F7 EF FF 9F C6 F1 FF = 14
    // 41 10 F3 41 0F 00 25 DC FF FF EC FA FF FF 2D E0 FF FF = 14
    // BF A5

    size_t offset = 0; // 当前解析位置

    offset += 5; // 0x59 0x53 [length] 共3B

    while (offset + 2 <= dataLen && offset + 2 <= data.size()) { // 至少剩余类型+长度字段
        uint8_t type = static_cast<uint8_t>(data[offset]);
        uint8_t len = static_cast<uint8_t>(data[offset + 1]);

        offset += 2; // 跳过类型和长度字段

        // 检查剩余数据长度是否足够
        if (offset + len > dataLen) {
            break;
        }

        const uint8_t* seq = reinterpret_cast<const uint8_t*>(data.data());
        const uint8_t* ptr = reinterpret_cast<const uint8_t*>(data.data()) + offset;

        auto ck = calPostureCk(seq + 2, dataLen + 3);

        if (ck.first == static_cast<uint8_t>(data[data.size() - 2]) && ck.second == static_cast<uint8_t>(data[data.size() - 1])) {
            switch (type) {
            case 0x10: // 加速度 (acc_x, acc_y, acc_z) - 12B
                if (len >= 12) {
                    acc_x = move(ptr, 4) * commonFactor;
                    acc_y = move(ptr + 4, 4) * commonFactor;
                    acc_z = move(ptr + 8, 4) * commonFactor;
                }
                break;

            case 0x20: // 角速度 (ang_x, ang_y, ang_z) - 12B
                if (len >= 12) {
                    ang_x = move(ptr, 4) * commonFactor;
                    ang_y = move(ptr + 4, 4) * commonFactor;
                    ang_z = move(ptr + 8, 4) * commonFactor;
                }
                break;

            case 0x30: // 磁力计数据
                if (len >= 12) {
                    mag_x = move(ptr, 4) * commonFactor;
                    mag_y = move(ptr + 4, 4) * commonFactor;
                    mag_z = move(ptr + 8, 4) * commonFactor;
                }
                break;

            case 0x31: // 原始数据
                if (len >= 12) {
                    raw_x = move(ptr, 4) * commonFactor;
                    raw_y = move(ptr + 4, 4) * commonFactor;
                    raw_z = move(ptr + 8, 4) * commonFactor;
                }
                break;

            case 0x40: // 姿态角 (pitch, roll, yaw) - 12B
                if (len >= 12) {
                    pitch = move(ptr, 4) * commonFactor;
                    roll = move(ptr + 4, 4) * commonFactor;
                    yaw = move(ptr + 8, 4) * commonFactor;
                }
                break;

            case 0x41: // 四元数 (quat_a, quat_b, quat_c, quat_d) - 16B
                if (len >= 16) {
                    quat_a = move(ptr, 4) * commonFactor;
                    quat_b = move(ptr + 4, 4) * commonFactor;
                    quat_c = move(ptr + 8, 4) * commonFactor;
                    quat_d = move(ptr + 12, 4) * commonFactor;
                }
                break;

            case 0x68: // 位置信息
                if (len >= 20) {
                    latitude = move(ptr, 8) * 0.0000000001;
                    longtitude = move(ptr + 8, 8) * 0.0000000001;
                    altitude = move(ptr + 16, 4) * 0.001;
                }
                break;

            case 0x70: // 速度信息
                if (len >= 12) {
                    vel_n = move(ptr, 4) * 0.001;
                    vel_e = move(ptr + 4, 4) * 0.001;
                    vel_d = move(ptr + 8, 4) * 0.001;
                }
                break;

            default:
                break;
            }

            offset += len; // 移动到下一个数据块
        }
        else {
            break;
        }
    }

    hs[POSTURE] = { acc_x, acc_y, acc_z, ang_x, ang_y, ang_z, mag_x, mag_y, mag_z, raw_x, raw_y, raw_z, pitch, roll, yaw };

    return {
        acc_x ,acc_y, acc_z, ang_x, ang_y, ang_z,
        mag_x, mag_y, mag_z, raw_x, raw_y, raw_z,
        pitch, roll, yaw, quat_a, quat_b, quat_c, quat_d,
        latitude, longtitude, altitude,
        vel_n, vel_e, vel_d
    };
}

std::vector<double> parseGuidance(const std::string& data)
{
    uint8_t check;
    bool receive = false;
    uint32_t move = 0;
    std::string guidanceStr;

    static double powerOnTime = 0; static double navTime = 0; static double statusWord = 0; static double faultWord = 0;

    static double inertiaPitch = 0; static double inertiaRoll = 0; static double inertiaHeading = 0;
    static double inertiaLongitude = 0; static double inertiaLatitude = 0; static double inertiaHeight = 0;
    static double inertiaEastSpeed = 0; static double inertiaNorthSpeed = 0;  static double inertiaSkySpeed = 0;
    static double xAxisAngularSpeedEncoded = 0; static double yAxisAngularSpeedEncoded = 0; static double zAxisAngularSpeedEncoded = 0;
    static double xAxisAccelerationEncoded = 0; static double yAxisAccelerationEncoded = 0; static double zAxisAccelerationEncoded = 0;

    static double combinedPitch = 0; static double combinedRoll = 0; static double combinedHeading = 0;
    static double combinedLongitude = 0;  static double combinedLatitude = 0;  static double combinedHeight = 0;
    static double combinedEastSpeed = 0;  static double combinedNorthSpeed = 0; static double combinedSkySpeed = 0;

    static double ppsTime = 0; static double satelliteTime = 0;
    static double satelliteLongitude = 0; static double satelliteLatitude = 0; static double satelliteHeight = 0;
    static double satelliteEastSpeed = 0; static double satelliteNorthSpeed = 0; static double satelliteSkySpeed = 0;
    static double positioningStatus = 0; static double availableSatellites = 0;  static double elevationAnomaly = 0;
    static double gpsWeek = 0;
    static double hdopValue = 0; static double vdopValue = 0;

    static double temperature = 0; static double humidity = 0;


    if (!data.empty()) {
        for (size_t i = 0; i < data.size(); i++) {
            if (!receive) {
                if (data[i] == 0x55 && static_cast<uint8_t>(data[i + 1]) == 0xAA) {
                    move = 1;
                    receive = true;
                    guidanceStr.clear();
                    guidanceStr.push_back(static_cast<uint8_t>(data[i]));
                }
                continue;
            }
            else {
                guidanceStr.push_back(static_cast<uint8_t>(data[i]));

                if (++move >= 160) {

                    if (move > data.size()) {
                        move = 0;
                        receive = false;
                        guidanceStr.clear();
                        continue;
                    }

                    const uint8_t* seq = reinterpret_cast<const uint8_t*>(guidanceStr.data());

                    check = calGuidanceCk(seq, 159);

                    if (check == static_cast<uint8_t>(data[move - 1])) {
                        if (guidanceStr.size() == sizeof(Guidance)) {
                            const Guidance* guidance = reinterpret_cast<const Guidance*>(guidanceStr.data());

                            powerOnTime = guidance->powerOnTime * 1.0 * 1e-3;
                            navTime = guidance->navTime * 1.0 * 1e-3;
                            statusWord = guidance->statusWord * 1.0;
                            faultWord = guidance->faultWord * 1.0;

                            inertiaPitch = guidance->inertiaPitch * 1.0 * 1e-6;
                            inertiaRoll = guidance->inertiaRoll * 1.0 * 1e-6;
                            inertiaHeading = guidance->inertiaHeading * 1.0 * 1e-6;

                            inertiaLongitude = guidance->inertiaLongitude * 1.0 * 1e-7;
                            inertiaLatitude = guidance->inertiaLatitude * 1.0 * 1e-7;
                            inertiaHeight = guidance->inertiaHeight * 1.0 * 1e-3;

                            inertiaEastSpeed = guidance->inertiaEastSpeed * 1.0 * 1e-4;
                            inertiaNorthSpeed = guidance->inertiaNorthSpeed * 1.0 * 1e-4;
                            inertiaSkySpeed = guidance->inertiaSkySpeed * 1.0 * 1e-4;

                            xAxisAngularSpeedEncoded = guidance->xAxisAngularSpeedEncoded;
                            yAxisAngularSpeedEncoded = guidance->yAxisAngularSpeedEncoded;
                            zAxisAngularSpeedEncoded = guidance->zAxisAngularSpeedEncoded;

                            xAxisAccelerationEncoded = guidance->xAxisAccelerationEncoded;
                            yAxisAccelerationEncoded = guidance->yAxisAccelerationEncoded;
                            zAxisAccelerationEncoded = guidance->zAxisAccelerationEncoded;

                            combinedPitch = guidance->combinedPitch * 1.0 * 1e-6;
                            combinedRoll = guidance->combinedRoll * 1.0 * 1e-6;
                            combinedHeading = guidance->combinedHeading * 1.0 * 1e-6;

                            combinedLongitude = guidance->combinedLongitude * 1.0 * 1e-7;
                            combinedLatitude = guidance->combinedLatitude * 1.0 * 1e-7;
                            combinedHeight = guidance->combinedHeight * 1.0 * 1e-3;

                            combinedEastSpeed = guidance->combinedEastSpeed * 1.0 * 1e-4;
                            combinedNorthSpeed = guidance->combinedNorthSpeed * 1.0 * 1e-4;
                            combinedSkySpeed = guidance->combinedSkySpeed * 1.0 * 1e-4;

                            ppsTime = guidance->ppsTime * 1.0 * 1e-3;
                            satelliteTime = guidance->satelliteTime * 1.0;

                            satelliteLongitude = guidance->satelliteLongitude * 1.0 * 1e-7;
                            satelliteLatitude = guidance->satelliteLatitude * 1.0 * 1e-7;
                            satelliteHeight = guidance->satelliteHeight * 1.0 * 1e-3;

                            satelliteEastSpeed = guidance->satelliteEastSpeed * 1.0 * 1e-4;
                            satelliteNorthSpeed = guidance->satelliteNorthSpeed * 1.0 * 1e-4;
                            satelliteSkySpeed = guidance->satelliteSkySpeed * 1.0 * 1e-2;

                            positioningStatus = guidance->positioningStatus * 1.0;
                            availableSatellites = guidance->availableSatellites * 1.0;
                            elevationAnomaly = guidance->elevationAnomaly * 1.0 * 1e-2;

                            gpsWeek = guidance->gpsWeek * 1.0;

                            hdopValue = guidance->hdopValue * 1.0 * 1e-2;
                            vdopValue = guidance->vdopValue * 1.0 * 1e-2;

                            temperature = guidance->temperature * 1.0;
                            humidity = guidance->humidity * 0.5;
                        }
                        move = 0;
                        receive = false;
                        guidanceStr.clear();
                    }
                }
            }
        }
    }

    hs["guidance"] = {
        inertiaPitch, inertiaRoll, inertiaHeading,
        inertiaLongitude,   inertiaLatitude,   inertiaHeight,
        inertiaEastSpeed,   inertiaNorthSpeed,    inertiaSkySpeed,

        xAxisAngularSpeedEncoded,   yAxisAngularSpeedEncoded,   zAxisAngularSpeedEncoded,
        xAxisAccelerationEncoded,   yAxisAccelerationEncoded,   zAxisAccelerationEncoded,

        combinedPitch,   combinedRoll,   combinedHeading,
        combinedLongitude,    combinedLatitude,    combinedHeight,
        combinedEastSpeed,    combinedNorthSpeed,   combinedSkySpeed,

        satelliteLongitude,   satelliteLatitude,   satelliteHeight,
        satelliteEastSpeed,   satelliteNorthSpeed,   satelliteSkySpeed,
    };

    return {
        powerOnTime,   navTime,   statusWord,   faultWord,
        inertiaPitch,   inertiaRoll,   inertiaHeading,
        inertiaLongitude,   inertiaLatitude,   inertiaHeight,
        inertiaEastSpeed,   inertiaNorthSpeed,    inertiaSkySpeed,

        xAxisAngularSpeedEncoded,   yAxisAngularSpeedEncoded,   zAxisAngularSpeedEncoded,
        xAxisAccelerationEncoded,   yAxisAccelerationEncoded,   zAxisAccelerationEncoded,

        combinedPitch,   combinedRoll,   combinedHeading,
        combinedLongitude,    combinedLatitude,    combinedHeight,
        combinedEastSpeed,    combinedNorthSpeed,   combinedSkySpeed,

        ppsTime,   satelliteTime,
        satelliteLongitude,   satelliteLatitude,   satelliteHeight,
        satelliteEastSpeed,   satelliteNorthSpeed,   satelliteSkySpeed,
        positioningStatus,   availableSatellites,    elevationAnomaly,
        gpsWeek,
        hdopValue,   vdopValue,

        temperature,   humidity, };
}

std::pair<std::string, std::vector<double>> parseDVL(const std::string& data) {
    std::vector<std::vector<std::string>> parsedData = constructDVLList(data);

    std::unordered_set<std::string> selectedIdentifiers = { "TS","BI", "BD", "BS" };

    std::string time;
    static double sound = 0;
    static double xSpeed = 0, ySpeed = 0, zSpeed = 0;
    static double altitude = 0;
    static double xAxis = 0, yAxis = 0, zAxis = 0;

    for (const auto& row : parsedData) {
        if (row.empty()) continue;
        const std::string& identifier = row[0];

        // 检查标识符是否在选择列表中
        if (selectedIdentifiers.find(identifier) != selectedIdentifiers.end()) {

            if (identifier == "TS" && row.size() >= 6) {
                time = row[1];
                sound = stringToDouble(row[5]);
            }

            // 特殊处理BI类型数据
            if (identifier == "BI" && row.size() >= 4) {
                // 提取并转换BI的速度数据
                xSpeed = stringToDouble(row[1]);
                ySpeed = stringToDouble(row[2]);
                zSpeed = stringToDouble(row[3]);

            }

            if (identifier == "BD" && row.size() >= 5) {
                altitude = stringToDouble(row[4]);
            }

            if (identifier == "BS" && row.size() >= 5) {
                xAxis = stringToDouble(row[1]);
                yAxis = stringToDouble(row[2]);
                zAxis = stringToDouble(row[3]);
            }
        }
    }

    hs[DVL] = { xSpeed, ySpeed, zSpeed, altitude, xAxis, yAxis, zAxis };

    return { time, {sound, xSpeed, ySpeed, zSpeed, altitude, xAxis, yAxis, zAxis } };
}

std::vector<double> parseSeatrac(const std::string& data)
{
    bool receive = false;
    uint32_t move = 0;
    std::string seatracStr;

    uint16_t ck = 0;

    double Timestamp_ms = 0.0;
    double EnvironmentSupply = 0.0;
    double EnvironmentTemperature = 0.0;
    double EnvironmentPressure = 0.0;
    double EnvironmentDepth = 0.0;
    double EnvironmentVos = 0.0;
    double AttitudeYaw = 0.0;
    double AttitudePitch = 0.0;
    double AttitudeRoll = 0.0;
    double MagCalBuf = 0.0;
    double MagCalValid = 0.0;
    double MagCalAge = 0.0;
    double MagCalFit = 0.0;
    double AccLimMinX = 0.0;
    double AccLimMinY = 0.0;
    double AccLimMinZ = 0.0;
    double AccLimMaxX = 0.0;
    double AccLimMaxY = 0.0;
    double AccLimMaxZ = 0.0;
    double AccRawX = 0.0;
    double AccRawY = 0.0;
    double AccRawZ = 0.0;
    double MagRawX = 0.0;
    double MagRawY = 0.0;
    double MagRawZ = 0.0;
    double GyroRawX = 0.0;
    double GyroRawY = 0.0;
    double GyroRawZ = 0.0;
    double AccCompX = 0.0f;
    double AccCompY = 0.0f;
    double AccCompZ = 0.0f;
    double MagCompX = 0.0f;
    double MagCompY = 0.0f;
    double MagCompZ = 0.0f;
    double GyroCompX = 0.0f;
    double GyroCompY = 0.0f;
    double GyroCompZ = 0.0f;

    if (!data.empty()) {
        for (size_t i = 0; i < data.size(); i++) {
            if (!receive) {
                if (data[i] == '$') {
                    receive = true;
                    move = 0;
                }
                continue;
            }
            else {
                seatracStr.push_back(data[i]);
                const uint8_t* seq = reinterpret_cast<const uint8_t*>(seatracStr.data());
                if (++move >= 105) {
                    ck = calSeatracCk(seq, seatracStr.size() - 2);

                    uint8_t crcLo = static_cast<uint8_t>(seatracStr[seatracStr.size() - 2]); // 低字节（前）
                    uint8_t crcHi = static_cast<uint8_t>(seatracStr[seatracStr.size() - 1]); // 高字节（后）
                    uint16_t ord = (crcHi << 8) | crcLo; // 组合为完整16位校验码（高字节<<8 + 低字节）

                    if (ck == ord) {
                        const Seatrac* seatrac = reinterpret_cast<const Seatrac*>(seq);

                        Timestamp_ms = seatrac->Timestamp_ms * 1.0 / 1000;
                        EnvironmentSupply = seatrac->EnvironmentSupply * 1.0 / 1000;
                        EnvironmentTemperature = seatrac->EnvironmentTemperature * 1.0 / 10;
                        EnvironmentPressure = seatrac->EnvironmentPressure * 1.0;
                        EnvironmentDepth = seatrac->EnvironmentDepth * 1.0;
                        EnvironmentVos = seatrac->EnvironmentVos * 1.0 / 10;

                        //姿态数据（偏移24开始）
                        AttitudeYaw = seatrac->AttitudeYaw * 1.0 / 10;
                        AttitudePitch = seatrac->AttitudePitch * 1.0 / 10;
                        AttitudeRoll = seatrac->AttitudeRoll * 1.0 / 10;

                        //磁校准数据（偏移30开始）
                        MagCalBuf = seatrac->MagCalBuf * 1.0;
                        MagCalValid = seatrac->MagCalValid * 1.0;
                        MagCalAge = seatrac->MagCalAge * 1.0;
                        MagCalFit = seatrac->MagCalFit * 1.0;//磁校准拟合度，偏移36（1字节）

                        //加速度限幅（偏移37开始）
                        AccLimMinX = seatrac->AccLimMinX * 1.0;
                        AccLimMinY = seatrac->AccLimMinY * 1.0;
                        AccLimMinZ = seatrac->AccLimMinZ * 1.0;
                        AccLimMaxX = seatrac->AccLimMaxX * 1.0;
                        AccLimMaxY = seatrac->AccLimMaxY * 1.0;
                        AccLimMaxZ = seatrac->AccLimMaxZ * 1.0;

                        //原始传感器数据（偏移49开始）
                        AccRawX = seatrac->AccRawX;
                        AccRawY = seatrac->AccRawY;
                        AccRawZ = seatrac->AccRawZ;
                        MagRawX = seatrac->MagRawX;
                        MagRawY = seatrac->MagRawY;
                        MagRawZ = seatrac->MagRawZ;
                        GyroRawX = seatrac->GyroRawX;
                        GyroRawY = seatrac->GyroRawY;
                        GyroRawZ = seatrac->GyroRawZ;

                        //补偿后加速度（32，偏移67开始）
                        AccCompX = seatrac->AccCompX;
                        AccCompY = seatrac->AccCompY;
                        AccCompZ = seatrac->AccCompZ;

                        //补偿后磁场与陀螺仪（32，偏移79开始）
                        MagCompX = seatrac->MagCompX;
                        MagCompY = seatrac->MagCompY;
                        MagCompZ = seatrac->MagCompZ;

                        GyroCompX = seatrac->GyroCompX;
                        GyroCompY = seatrac->GyroCompY;
                        GyroCompZ = seatrac->GyroCompZ;

                    }
                    else {

                    }
                }
            }
        }
    }

    hs[SEATRAC] = { AttitudeYaw, AttitudePitch, AttitudeRoll, AccCompX, AccCompY, AccCompZ, GyroCompX, GyroCompY, GyroCompZ };

    return {
        Timestamp_ms,

        EnvironmentSupply, EnvironmentTemperature, EnvironmentPressure, EnvironmentDepth, EnvironmentVos,
        AttitudeYaw, AttitudePitch, AttitudeRoll,

        MagCalBuf, MagCalValid, MagCalAge, MagCalFit,
        AccLimMinX, AccLimMinY, AccLimMinZ, AccLimMaxX, AccLimMaxY, AccLimMaxZ,
        AccRawX, AccRawY, AccRawZ, MagRawX, MagRawY, MagRawZ, GyroRawX, GyroRawY, GyroRawZ,

        AccCompX, AccCompY, AccCompZ,
        MagCompX, MagCompY, MagCompZ,
        GyroCompX, GyroCompY, GyroCompZ
    };
}

std::vector<double> parseGPS(const std::string& data) {
    // 初始化返回值（与原逻辑保持一致的字段顺序）
    double utcTime = 0;
    double status = 0;
    double latitude = 0;
    double latDir = 0;
    double longitude = 0;
    double lonDir = 0;
    double speedKnots = 0;
    double course = 0;
    double utcDate = 0;
    double magneticVariation = 0;
    double varDir = 0;
    double indicator = 0;

    const std::string frameStart = "$GPRMC";

    // 查找最后一帧GPRMC的起始位置（因OnLastMessage确保存在）
    size_t frameStartPos = data.find(frameStart);
    if (frameStartPos == std::string::npos) {
        // 理论上不会走到这里，因上游已确保存在GPRMC帧
        return { utcTime, status, latitude, latDir, longitude, lonDir,
                speedKnots, course, utcDate, magneticVariation, varDir, indicator };
    }

    // 定位到字段起始位置（跳过"$GPRMC,"）
    size_t fieldStartPos = frameStartPos + frameStart.length();
    if (fieldStartPos >= data.length() || data[fieldStartPos] != ',') {
        return { utcTime, status, latitude, latDir, longitude, lonDir,
                speedKnots, course, utcDate, magneticVariation, varDir, indicator };
    }
    fieldStartPos++; // 跳过逗号

    // 查找校验和分隔符"*"
    size_t checksumSepPos = data.find('*', fieldStartPos);
    if (checksumSepPos == std::string::npos) {
        return { utcTime, status, latitude, latDir, longitude, lonDir,
                speedKnots, course, utcDate, magneticVariation, varDir, indicator };
    }

    // 提取校验和并验证（保持数据可靠性）
    std::string checksumStr = data.substr(checksumSepPos + 1, 2);
    uint8_t claimedCk = hexStrToValue(checksumStr);

    // 计算校验和（从$后一位到*前一位）
    size_t calcStart = frameStartPos + 1; // 跳过'$'
    size_t calcEnd = checksumSepPos;
    uint8_t calcCk = calGPSCK(data, calcStart, calcEnd);

    // 校验和不匹配则返回空值
    if (claimedCk != calcCk) {
        return { utcTime, status, latitude, latDir, longitude, lonDir,
                speedKnots, course, utcDate, magneticVariation, varDir, indicator };
    }

    // 提取字段内容（从第一个字段到*之前）
    std::string fields = data.substr(fieldStartPos, checksumSepPos - fieldStartPos);

    // 按逗号分割字段
    std::vector<std::string> parts;
    std::stringstream ss(fields);
    std::string token;
    while (std::getline(ss, token, ',')) {
        parts.push_back(token);
    }

    // 解析字段（NMEA RMC标准格式）
    // 0=UTC时间, 1=状态(A=有效,V=无效), 2=纬度, 3=南北半球(N/S),
    // 4=经度, 5=东西半球(E/W), 6=速度(节), 7=方位角,
    // 8=UTC日期, 9=磁偏角, 10=磁偏角方向(E/W), 11=模式指示器
    if (parts.size() > 0) utcTime = strToDouble(parts[0]);
    if (parts.size() > 1) status = (parts[1] == "A") ? 1 : 0;  // 有效=1,无效=0
    if (parts.size() > 2) latitude = strToDouble(parts[2]);
    if (parts.size() > 3) latDir = (parts[3] == "S") ? -1 : 1;   // 南半球=-1,北半球=1
    if (parts.size() > 4) longitude = strToDouble(parts[4]);
    if (parts.size() > 5) lonDir = (parts[5] == "W") ? -1 : 1;   // 西经=-1,东经=1
    if (parts.size() > 6) speedKnots = strToDouble(parts[6]);
    if (parts.size() > 7) course = strToDouble(parts[7]);
    if (parts.size() > 8) utcDate = strToDouble(parts[8]);
    if (parts.size() > 9) magneticVariation = strToDouble(parts[9]);
    if (parts.size() > 10) varDir = (parts[10] == "W") ? -1 : 1; // 西偏=-1,东偏=1
    if (parts.size() > 11) indicator = strToDouble(parts[11]);

    hs[GPS] = { latDir, longitude, lonDir };

    return { utcTime, status, latitude, latDir, longitude, lonDir,
            speedKnots, course, utcDate, magneticVariation, varDir, indicator };
}

std::vector<double> parseHumiture(const std::string& data)
{
    double temperature = 0;
    double humidity = 0;
    bool receive = false;
    uint16_t ck = 0;

    uint32_t move = 0;

    std::string humitureStr;

    if (!data.empty()) {
        for (size_t i = 0; i < data.size(); i++) {
            if (!receive) {
                if (i + 1 < data.size()) {
                    if (data[i] == 0x01 && data[i + 1] == 0x03) {
                        move = 1;
                        receive = true;
                        humitureStr.clear();
                        humitureStr.push_back(static_cast<uint8_t>(data[i]));
                    }
                }
                continue;
            }
            else {
                humitureStr.push_back(static_cast<uint8_t>(data[i]));
                if (++move >= 9) {
                    if (move > data.size()) {
                        move = 0;
                        receive = false;
                        humitureStr.clear();
                        continue;
                    }

                    const uint8_t* seq = reinterpret_cast<const uint8_t*>(humitureStr.data());
                    ck = calHumitureCk(seq, 7);

                    uint8_t c1 = data[move - 2];
                    uint8_t c2 = data[move - 1];

                    uint16_t ord = c2 << 8 | c1;

                    if (ck == ord) {
                        if (humitureStr.size() == sizeof(Humiture)) {
                            const Humiture* tem = reinterpret_cast<const Humiture*>(humitureStr.data());

                            temperature = 1.0 * (tem->tem_high << 8 | tem->tem_low) / 100;
                            humidity = 1.0 * (tem->hum_high << 8 | tem->hum_low) / 100;
                        }
                        move = 0;
                        receive = false;
                        humitureStr.clear();
                    }
                }
            }
        }
    }

    hs[HUMITURE] = { temperature, humidity };

    return { temperature, humidity };
}

std::string generate_csv_header(const std::vector<std::string>& fields) {
    if (fields.empty()) {
        return "";
    }

    std::ostringstream oss;  // 用于构建字符串
    // 使用ostream_iterator，自动在每个元素后添加","
    std::ranges::copy(
        fields,
        std::ostream_iterator<std::string>(oss, ", ")  // 分隔符为逗号
    );

    std::string header = oss.str();
    // 移除最后一个多余的逗号
    if (!header.empty()) {
        header.pop_back();
        header.pop_back();
    }

    return header;
}


std::string get_header(const std::string& name)
{
    std::string header;
    if (name == COMPASS) {
        header = "timestamp,pitch,roll,heading";
        return header;
    }
    else if (name == DEPTH) {
        header = "timestamp,pressure";
        return header;
    }
    else if (name == DVL) {
        header = "timestamp, TS, BI, BD, BS";
        return header;
    }
    else if (name == GUIDANCE) {
        std::vector<std::string> name_lists = { "timestamp", "powerOnTime", "navTime", "statusWord", "faultWord",
            "inertiaPitch", "inertiaRoll", "inertiaHeading", "inertiaLongitude", "inertiaLatitude", "inertiaHeight",
            "inertiaEastSpeed", "inertiaNorthSpeed", "inertiaSkySpeed",
            "xAxisAngularSpeedEncoded", "yAxisAngularSpeedEncoded", "zAxisAngularSpeedEncoded",
            "xAxisAccelerationEncoded", "yAxisAccelerationEncoded", "zAxisAccelerationEncoded",
            "combinedPitch", "combinedRoll", "combinedHeading", "combinedLongitude", "combinedLatitude", "combinedHeight",
            "combinedEastSpeed", "combinedNorthSpeed", "combinedSkySpeed", "ppsTime",
            "satelliteTime",
            "satelliteLongitude", "satelliteLatitude", "satelliteHeight",
            "satelliteEastSpeed", "satelliteNorthSpeed", "satelliteSkySpeed",
            "positioningStatus",  "availableSatellites", "elevationAnomaly",
            "gpsWeek",
            "hdopValue", "vdopValue",
            "temperature", "humidity"
        };

        header = generate_csv_header(name_lists);
        return header;
    }
    else if (name == POSTURE) {
        std::vector<std::string> name_lists = { "timestamp",
            "xAxisAccelerationEncoded", "yAxisAccelerationEncoded", "zAxisAccelerationEncoded",
            "xAxisAngularSpeedEncoded", "yAxisAngularSpeedEncoded", "zAxisAngularSpeedEncoded",
            "xAxisMagneticFieldNormalized", "yAxisMagneticFieldNormalized", "zAxisMagneticFieldNormalized"
            "xAxisMgneticFieldRaw", "yAxisMagneticFieldRaw", "zAxisMagneticFieldRaw",
            "pitch", "roll", "yaw",
            "quaternionW", "quaternionX", "quaternionY", "quaternionZ"
        };

        header = generate_csv_header(name_lists);
        return header;
    }
    else if (name == HUMITURE) {
        header = "timestamp, temperature, humiture";
        return "";
    }
    else if (name == SEATRAC) {
        std::vector<std::string> name_lists = { "timestamp",
            "EnvironmentSupply", "EnvironmentTemperature",
            "EnvironmentPressure", "EnvironmentDepth", "EnvironmentVos",
            "AttitudeYaw", "AttitudePitch", "AttitudeRoll" ,
            "MagCalBuf", "MagCalValid", "MagCalAge", "MagCalFit",
            "AccLimMinX", "AccLimMinY", "AccLimMinZ",
            "AccLimMaxX", "AccLimMaxY", "AccLimMaxZ",
            "AccRawX", "AccRawY", "AccRawZ",
            "MagRawX", "MagRawY", "MagRawZ",
            "GyroRawX", "GyroRawY", "GyroRawZ",
            "AccCompX", "AccCompY", "AccCompZ",
            "MagCompX", "MagCompY", "MagCompZ",
            "GyroCompX", "GyroCompY", "GyroCompZ"
        };

        header = generate_csv_header(name_lists);
        return header;
    }
    return std::string();
}

std::string get_last_message(const std::string& name, const std::string& data)
{
    if (name == COMPASS) {
        return getLastCompass(data);
    }
    else if (name == DEPTH) {
        return getLastDepth(data);
    }
    else if (name == DVL) {
        return getLastDVL(data);
    }
    else if (name == GUIDANCE) {
        return getLastGuidance(data);
    }
    else if (name == POSTURE) {
        return getLastPosture(data);
    }
    else if (name == HUMITURE) {
        return getLastHumiture(data);
    }
    else if (name == SEATRAC) {
        return getLastSeatrac(data);
    }
    return std::string();
}


std::string getLastCompass(const std::string& data) {
    const char frameStart = 0x68;        // 帧头
    const size_t frameLength = 14;       // 每帧14字节

    std::string lastFrame;

    size_t pos = 0;
    while (pos + frameLength <= data.size()) {  // 确保剩余数据足够一帧
        // 查找帧头
        pos = data.find(frameStart, pos);
        if (pos == std::string::npos) break;  // 没有更多帧头

        // 检查是否有完整的14字节帧
        if (pos + frameLength > data.size()) break;  // 剩余数据不足一帧

        // 提取完整帧
        lastFrame = data.substr(pos, frameLength);

        // 移动到下一位置继续查找
        pos += 1;
    }
    return lastFrame;
}

//std::string getLastDepth(const std::string& data) {
//    size_t startPos = 0;
//    size_t endPos = 0;
//    std::string lastFrame;
//
//    // 查找字面量"\\n"（两个字符：反斜杠+字母n）
//    while ((endPos = data.find("\\n", startPos)) != std::string::npos) {
//        // 提取从startPos到"\\n"前的内容（不含"\\n"）
//        std::string message = data.substr(startPos, endPos - startPos);
//
//        // 去除末尾可能的"\\r"（如果有的话）
//        if (message.size() >= 2 && message.substr(message.size() - 2) == "\\r") {
//            message = message.substr(0, message.size() - 2);
//        }
//
//        lastFrame = message;
//        startPos = endPos + 2; // 跳过"\\n"（两个字符）
//    }
//
//    // 如果没有找到"\\n"，则取整个字符串（并处理可能的"\\r"）
//    if (lastFrame.empty()) {
//        lastFrame = data;
//        if (lastFrame.size() >= 2 && lastFrame.substr(lastFrame.size() - 2) == "\\r") {
//            lastFrame = lastFrame.substr(0, lastFrame.size() - 2);
//        }
//    }
//
//    return lastFrame;
//}

std::string getLastDepth(const std::string& data) {
    size_t startPos = 0;
    size_t endPos = 0;
    std::string lastFrame;      // 存储最后一个完整帧

    // 循环查找换行符 '\n'（ASCII 0x0A）
    while ((endPos = data.find('\n', startPos)) != std::string::npos) {
        std::string message = data.substr(startPos, endPos - startPos);
        lastFrame = message;
        startPos = endPos + 1; // 跳过换行符

    }

    return lastFrame;
}

std::string getLastGuidance(const std::string& data) {
    const uint8_t firstFrame = 0x55;
    const uint8_t secondFrame = 0xAA;
    std::string currentMessage; // 当前正在构建的消息

    std::string lastFrame;

    bool receive = false;
    uint8_t frameLength = 160;

    for (size_t i = 0; i < data.size(); i++) {
        if (!receive) {

            // 检测帧头
            if (i + 1 < data.size() && data[i] == firstFrame && static_cast<uint8_t>(data[i + 1]) == secondFrame) {
                // 如果之前有未完成的消息，丢弃它
                if (!currentMessage.empty()) {
                    currentMessage.clear();
                }

                receive = true;
                currentMessage.push_back(data[i]);    // 添加帧头第一个字节
            }
            continue;
        }
        else {
            currentMessage.push_back(static_cast<uint8_t>(data[i]));


            // 检查是否收集到完整的帧（帧头+长度+数据+校验和）
            if (currentMessage.size() >= frameLength) {
                lastFrame = currentMessage;

                // 重置状态，准备下一个帧
                receive = false;
                currentMessage.clear();
                frameLength = 0;
            }
        }
    }

    return lastFrame;
}

std::string getLastDVL(const std::string& data) {

    // 解析数据
    std::vector<std::vector<std::string>> parsedData = constructDVLList(data);

    // 指定要输出的标识符
    std::unordered_set<std::string> selectedIdentifiers = { "TS", "BI", "BD", "BS" };

    std::stringstream resultSS;

    // 存储每个标识符的最后一条记录
    std::unordered_map<std::string, std::vector<std::string>> lastRecords;

    // 遍历所有解析的数据，保存每个标识符的最后一条记录
    for (const auto& row : parsedData) {
        if (row.empty()) continue;
        const std::string& identifier = row[0];

        // 如果是我们关注的标识符，则更新其最后一条记录
        if (selectedIdentifiers.find(identifier) != selectedIdentifiers.end()) {
            lastRecords[identifier] = row;
        }
    }

    // 构建结果字符串
    bool hasData = false;

    for (const auto& identifier : selectedIdentifiers) {
        auto it = lastRecords.find(identifier);
        if (it != lastRecords.end()) {
            const auto& row = it->second;
            hasData = true;

            // 输出整行数据
            resultSS << ":";
            for (size_t i = 0; i < row.size(); ++i) {
                resultSS << row[i];
                if (i < row.size() - 1) {
                    resultSS << ", ";
                }
            }
            resultSS << "\n";
        }
    }

    if (!hasData) {
        resultSS << "未找到选定类型的记录";
    }

    return resultSS.str();
}

std::string getLastPosture(const std::string& data)
{
    const uint8_t firstFrame = 0x59;
    const uint8_t secondFrame = 0x53;
    std::string currentMessage; // 当前正在构建的消息

    std::string lastFrame;

    bool receive = false;
    uint8_t frameLength = 0;
    size_t step = 0;

    // 59 53 B1 71 3C = 5
    // 10 0C 27 2F 01 00 AE 0C FE FF 6E C1 95 00 
    // 20 0C AC 79 04 00 AB 5D 01 00 E5 D6 FF FF 
    // 40 0C E8 72 F9 FF CC 7C F3 FF C1 78 EC FF 
    // 41 10 E0 41 0F 00 E3 E3 FF FF AE F1 FF FF 46 D4 FF FF 
    // 14 + 14 + 14 = 60
    // 59 F8 = 2

    for (size_t i = 0; i < data.size(); i++) {
        if (!receive) {
            // 检测帧头
            if (i + 1 < data.size() && data[i] == firstFrame && data[i + 1] == secondFrame) {
                // 如果之前有未完成的消息，丢弃它
                if (!currentMessage.empty()) {
                    currentMessage.clear();
                }

                receive = true;
                step = i;
                currentMessage.push_back(data[i]);    // 添加帧头第一个字节
            }
            continue;
        }
        else {
            currentMessage.push_back(static_cast<uint8_t>(data[i]));

            // 读取帧长度（在帧头后第3个字节）
            if (i == step + 4) {
                frameLength = data[i];
            }

            // 检查是否收集到完整的帧（帧头+长度+数据+校验和）
            if (currentMessage.size() >= frameLength + 7) {
                lastFrame = currentMessage;

                // 重置状态，准备下一个帧
                receive = false;
                currentMessage.clear();
                frameLength = 0;
                step = 0;
            }
        }
    }

    return lastFrame;
}

std::string getLastSeatrac(const std::string& data)
{
    // 10 07 8D 48 10 00 00 00 00 00 B9 30 C2 00 08 00 00 00 00 00 00 00 48 0D E3 FD 0D FD 32 03 03 FF 2B 04 00 00 5E F2 73

    std::string currentMessage;
    const uint16_t frameLength = 211;

    std::string lastFrame;

    bool receive = false;

    for (size_t i = 0; i < data.size(); i++) {
        if (!receive) {
            if (data[i] == '$') {
                if (!currentMessage.empty()) {
                    currentMessage.clear();
                }
                currentMessage.push_back(data[i]);
                receive = true;
            }
            continue;
        }
        else {
            currentMessage.push_back(data[i]);

            if (currentMessage.size() >= frameLength) {

                //wxLogInfo("currentMessage %d", currentMessage.size());
                currentMessage = hexStringToBytes(currentMessage);
                lastFrame = currentMessage;
                receive = false;
                currentMessage.clear();
            }
        }
    }

    //wxLogInfo("lastFrame %s, %d", toHexString(lastFrame), lastFrame.size());

    return lastFrame;
}

// 查找并返回最新的完整GPRMC帧
std::string getLastGPS(const std::string& data)
{
    static std::string currentBuffer;
    static std::string cachedFrame;  // 始终保存最后一个完整帧

    // 将新数据添加到缓冲区
    currentBuffer += data;

    // 定义GPRMC帧的起始和结束标志
    const std::string startMarker = "$GPRMC";
    const std::string endMarker = "\r\n";

    size_t lastStart = std::string::npos;
    size_t lastEnd = std::string::npos;
    size_t pos = 0;

    // 遍历缓冲区，查找所有完整帧，定位最后一个完整帧
    while (pos < currentBuffer.size())
    {
        size_t start = currentBuffer.find(startMarker, pos);
        if (start == std::string::npos)
            break;  // 没有更多起始标志，退出循环

        // 从起始标志后查找结束标志
        size_t end = currentBuffer.find(endMarker, start + startMarker.length());
        if (end != std::string::npos)
        {
            // 更新最后一个完整帧的位置
            lastStart = start;
            lastEnd = end + endMarker.length();
            pos = lastEnd;  // 继续查找后续可能的帧
        }
        else
        {
            // 未找到结束标志，移动到下一个字符
            pos = start + 1;
        }
    }

    // 如果找到新的最后一个完整帧
    if (lastStart != std::string::npos && lastEnd != std::string::npos)
    {
        // 提取最后一个完整帧并更新缓存
        cachedFrame = currentBuffer.substr(lastStart, lastEnd - lastStart);

        // 关键修正：currentBuffer只保留最后一个完整帧之后的内容
        // 而不是清空，确保后续数据可以接续处理
        currentBuffer = currentBuffer.substr(lastEnd);

        return cachedFrame;
    }

    // 没有找到新的完整帧时，返回缓存的最后一个完整帧
    return cachedFrame;
}

std::string getLastHumiture(const std::string& data)
{
    const char frameStart = 0x01;
    const size_t frameLength = 9;

    std::string lastFrame;

    size_t pos = 0;
    while (pos + frameLength <= data.size()) {
        pos = data.find(frameStart, pos);
        if (pos == std::string::npos)break;

        if (pos + frameLength > data.size())break;

        lastFrame = data.substr(pos, frameLength);

        pos += 1;
    }

    return lastFrame;
}

// 传感器数据发送逻辑（仅处理数据，网络请求交给CurlClient）
void post_sensor_data(const std::string& name, const std::string& url) {
    // 步骤1：线程安全读取传感器数据
    std::vector<double> data;
    {
        std::lock_guard<std::mutex> lock(hs_mutex);
        auto it = hs.find(name);
        if (it == hs.end() || it->second.empty()) {
            std::println("❌ 传感器 {} 无数据可发送", name);
            return;
        }
        data = it->second;  // 拷贝数据，缩短锁持有时间
    }



    // 步骤2：生成JSON数据（数据格式转换）
    std::string json_data = transfer_json(name, data);
    if (json_data.empty()) {
        std::println("❌ 传感器 {} JSON生成失败", name);
        return;
    }

    // 步骤3：使用CurlClient发送请求（解耦核心：网络操作交给独立类）
    CurlClient client;
    client.setUrl(url);
    client.setTimeOut(1);  // 1秒超时
    // client.setSslVerify(true);  // 生产环境启用HTTPS验证

    bool sendSuccess = client.postJson(json_data);

    // 步骤4：处理发送结果
    if (sendSuccess) {
        std::println("✅ 传感器 {} 发送成功，响应: {}", name, client.getResponse());
        std::println("📊 HTTP状态码: {}", client.getHttpCode());

        // 发送成功后清空数据
        std::lock_guard<std::mutex> lock(hs_mutex);
        hs[name].clear();
    }
    else {
        std::println("❌ 传感器 {} 发送失败，原因: {}", name, client.getError());
        if (client.getHttpCode() != 0) {
            std::println("⚠️ HTTP状态码: {}", client.getHttpCode());
        }
        // 发送失败可在此添加重试逻辑
    }
}

void post_timeout_data(const std::string& name, const std::string& url)
{
    nlohmann::json j;
    j["timeout"] = name;

    std::string json_data = j.dump(4);
    if (json_data.empty()) {
        std::println("❌ 传感器 {} JSON生成失败", name);
        return;
    }

    CurlClient client;
    client.setUrl(url);
    client.setTimeOut(1);  // 1秒超时

    bool sendSuccess = client.postJson(json_data);

    // 步骤4：处理发送结果
    if (sendSuccess) {
        std::println("✅ 传感器 {} 发送成功，响应: {}", name, client.getResponse());
        std::println("📊 HTTP状态码: {}", client.getHttpCode());
    }
    else {
        std::println("❌ 传感器 {} 发送失败，原因: {}", name, client.getError());
        if (client.getHttpCode() != 0) {
            std::println("⚠️ HTTP状态码: {}", client.getHttpCode());
        }
        // 发送失败可在此添加重试逻辑
    }
}

std::string hexStringToBytes(const std::string& hex_str)
{
    std::string bytes;
    bytes.push_back('$');
    for (size_t i = 1; i < hex_str.length(); i += 2) {
        std::string byte_str = hex_str.substr(i, 2);
        uint8_t byte = static_cast<uint8_t>(std::stoi(byte_str, nullptr, 16));
        bytes.push_back(byte);
    }
    return bytes;
}

std::string send_remote_x010_data()
{
    NAV_Send_Struct send_cmd;
    memset(&send_cmd, 0, sizeof(send_cmd));

    send_cmd.sync_char = '#';
    send_cmd.msg_id = NAV_CID_E::CID_NAV_QUERY_SEND;
    send_cmd.dest_id = BID_E::BEACON_ID_2;

    send_cmd.query_flags.qry_depth = 1;
    send_cmd.query_flags.qry_attitude = 1;
    send_cmd.query_flags.reserved_4_6 = 0;

    send_cmd.packet_len = 0;
    send_cmd.end_char = { '\r', '\n' };


    size_t checksum_data_len = offsetof(NAV_Send_Struct, checksum) - offsetof(NAV_Send_Struct, msg_id);
    const uint8_t* checksum_data = reinterpret_cast<const uint8_t*>(&send_cmd.msg_id);
    send_cmd.checksum = calSeatracCk(checksum_data, checksum_data_len);

    std::string send_data(reinterpret_cast<const char*>(&send_cmd), sizeof(send_cmd));

    return send_data;
}

std::vector<double> receive_remote_x010_data(const std::string& data)
{
    if (data.size() < sizeof(NAV_Receive_Struct)) {
        std::println("接收数据长度不足: {}字节", data.size());
    }

    double dest_id;
    double src_id;
    double flags = 0;
    double attitude_yaw = 0;
    double attitude_pitch = 0;
    double depth_local = 0;
    double vos = 0;
    double rssi = 0;

    const NAV_Receive_Struct* resp = reinterpret_cast<const NAV_Receive_Struct*>(data.data());

    if (resp->sync_char != '$' ||
        resp->end_char.at(0) != '\r' ||
        resp->end_char.at(1) != '\n') {
        std::println("响应格式错误: 同步字符或通配符不匹配");
    }

    if (resp->msg_id != NAV_CID_E::CID_NAV_QUERY_RESP) {
        if (resp->msg_id != NAV_CID_E::CID_NAV_QUERY_RESP) {
            std::println(
                "消息ID不匹配: 收到0x{:02X}, 预期0x{:02X}",
                static_cast<uint8_t>(resp->msg_id),  // 显式转换为整数
                static_cast<uint8_t>(NAV_CID_E::CID_NAV_QUERY_RESP)  // 显式转换为整数
            );
        }
    }

    size_t checksum_dat_len = offsetof(NAV_Receive_Struct, checksum) - offsetof(NAV_Receive_Struct, msg_id);
    const uint8_t* checksum_data = reinterpret_cast<const uint8_t*>(&resp->msg_id);
    uint16_t crc = calSeatracCk(checksum_data, checksum_dat_len);

    if (crc != resp->checksum) {
        std::println("校验和错误: 计算0x{:04X}, 接收0x{:04X}", crc, resp->checksum);
    }

    dest_id = static_cast<int>(resp->aco_fix.dest_id) * 1.0;
    src_id = static_cast<int>(resp->aco_fix.src_id) * 1.0;

    flags = resp->aco_fix.flags * 1.0;
    attitude_yaw = resp->aco_fix.attitude_yaw / 10.0;
    attitude_pitch = resp->aco_fix.attitude_pitch / 10.0;
    depth_local = resp->aco_fix.depth_local / 10.0;
    vos = resp->aco_fix.vos / 10.0;
    rssi = resp->aco_fix.rssi / 10.0;

    return { dest_id, src_id, flags, attitude_yaw, attitude_yaw, depth_local, vos, rssi };
}

int64_t move(const uint8_t* data, int step) {
    int64_t mov = 0;
    for (int i = 0; i < step; i++) {
        // 小端序拼接：低位字节左移i*8位
        mov += static_cast<int64_t>(data[i]) << (8 * i);
    }
    // 对32位数据进行符号扩展（若step=4）
    if (step == 4) {
        mov = static_cast<int32_t>(mov); // 截断为32位，自动处理符号扩展
    }
    return mov;
}

std::vector<std::string> splitDVLList(const std::string& line)
{
    std::vector<std::string> result;
    std::stringstream ss(line);
    std::string item;

    while (std::getline(ss, item, ',')) {
        // 去除前导和尾随空格
        size_t start = item.find_first_not_of(" \t");
        size_t end = item.find_last_not_of(" \t");

        if (start != std::string::npos && end != std::string::npos) {
            result.push_back(item.substr(start, end - start + 1));
        }
        else {
            result.push_back(""); // 空字段
        }
    }

    return result;
}

std::vector<std::vector<std::string>> constructDVLList(const std::string& data)
{
    std::vector<std::vector<std::string>> result;
    std::regex pattern(R"(:[A-Z]{2}.*)"); // 匹配以:XX开头的行

    // 遍历所有匹配的行
    auto words_begin = std::sregex_iterator(data.begin(), data.end(), pattern);
    auto words_end = std::sregex_iterator();

    for (std::sregex_iterator i = words_begin; i != words_end; ++i) {
        std::smatch match = *i;
        std::string line = match.str();

        // 提取标识符（去掉冒号）
        std::string identifier = line.substr(1, 2);

        // 提取剩余部分（从第4个字符开始，跳过":XX,"）
        std::string values_part = line.substr(4);

        // 分割剩余部分
        std::vector<std::string> values = splitDVLList(values_part);

        // 构建最终结果：[标识符, 值1, 值2, ...]
        std::vector<std::string> row;
        row.push_back(identifier);
        row.insert(row.end(), values.begin(), values.end());

        result.push_back(row);
    }

    return result;
}

std::vector<uint8_t> hexStringToByteses(const std::string& hex)
{
    std::vector<uint8_t> bytes;
    for (size_t i = 0; i < hex.length(); i += 2) {
        // 跳过空格
        if (hex[i] == ' ') {
            i++;
            if (i >= hex.length()) break;
        }
        // 提取两个字符作为一个十六进制值
        std::string byteString = hex.substr(i, 2);
        uint8_t byte = static_cast<uint8_t>(std::stoi(byteString, nullptr, 16));
        bytes.push_back(byte);
    }
    return bytes;
}

uint8_t calculateXOR(const uint8_t* data, int len)
{
    uint8_t ck = 0;
    for (int i = 0; i < len - 1; i++) {
        ck = ck ^ data[i];
    }
    return ck;
}

double stringToDouble(const std::string& str)
{
    try {
        // 直接转换，保留符号（+/-）
        return std::stod(str);
    }
    catch (const std::exception& e) {
        std::cerr << "Error converting '" << str << "' to double:" << e.what() << std::endl;
        return 0.0;
    }
}

double strToDouble(const std::string& str)
{
    if (str.empty()) return 0.0;
    try {
        return std::stod(str);
    }
    catch (...) {
        return 0.0;
    }
}

uint8_t hexStrToValue(const std::string& hexStr)
{
    uint32_t value;
    std::stringstream ss;
    ss << std::hex << hexStr;
    ss >> value;
    return static_cast<uint8_t>(value);
}
